clear all;
close all;

J=2;
kp=1.2;
kint=3.1;
kd=0.08;
beta=1.0;
lamb=1.2;
PHI=0.01;

Qk=[10;0];% ode45不能直接用矩阵，得变成向量进行操作；依次是phi,th,fi,h
Zk=[0;0;0];


T=0.001;
for k=1:1:12000
  
    time(k)=k*T;

    thd(k)=0;     
%     dthd(k)=cos(k*T);    
%     ddthd(k)=-sin(k*T);

    th(k)=Qk(1);dth(k)=Qk(2);

    if(k*T>=5)
        dt=2*cos(2*pi/3*k*T);
    else
        dt=0;
    end

    s=Zk(1);ds=Zk(2);gama=Zk(3);
    
    e = thd(k)-th(k);
    de = -dth(k);
    
    vt(k)=J/kd*(-beta*ds+kp*de+kint*e+lamb*s+gama*saturate(ds/PHI,-1,1));
     
    U=vt(k);

    tSpan = [0 T];
	[tt,Qq] = ode45(@(t,x) plant1(t,x,U,dt),tSpan,Qk);
    Qk = Qq(length(Qq),:);
   	[tt1,Zq] = ode45(@(t,x) plants1(t,x,dt),tSpan,Zk);
    Zk = Zq(length(Zq),:);
                              
                                   
     
end

figure(1);
plot(time,th,'k',time,thd,'r--');
xlabel('time(s)');ylabel('th');
legend('th','thd');
grid on;

figure(2);
plot(time,vt,'k');
xlabel('time(s)');ylabel('U');
legend('U');
grid on;
 